#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <assert.h>
#include <zmq.h>
#include <string>
#include<iostream>
#include<vector>
#include <iterator>
#include<iomanip>
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/time.h>
#include <sys/resource.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <netdb.h>
#include <stdio.h>
#include <unistd.h>
#include <errno.h>
#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <time.h>
#include <string>
using namespace std;
#define PORT "1917"
#define FLIGHTGEAR_IP "127.0.0.1"
//fgfs --generic=socket,in,5,127.0.0.1,1917,udp,suyu_input --aircraft=mibs --httpd=5400
int addr_len, bytes_read;
char recv_data[1024];
char send_data[1024];
struct addrinfo hints, *servinfo, *p;
int rv, sockfd;
int msg_len = 0;
//定义数据总长，注意！！！！！！总长不对一定会导致FG通讯失败！！！！！！（苏雨）
int data_len = 0;
//定义初始化开关，如果开关置于1，则定义数组并初始化，这是为了防止先创建飞机再启动节点，导致数组未定义的错误！（苏雨） 
int init = 1;
//定义数据选择开关，如果为0则开关处于中间，如果为1则数据进入lat，如果为2则数据进入lon，如果为3则进入alt，如果为4则进入tas，如果为5则进入trk（苏雨）
int latlonalt = 0;
//c里面没有向量，c++有（苏雨）
vector<double> lat_save; 
vector<double> lon_save;
vector<double> alt_save;
vector<double> tas_save;
vector<double> trk_save;
vector<double> lat;
vector<double> lon;
vector<double> alt;
vector<double> tas;
vector<double> trk;
//定义需要显示位置信息的飞机序号（苏雨）
int aircraft_order = 0;

//定义udp连接函数（苏雨）
int udp_connect_setup(void) 
{
    memset(&hints, 0, sizeof(hints));
    hints.ai_family = AF_INET;
    hints.ai_socktype = SOCK_DGRAM;
    /*
        error check get addr info, else put into servinfo
        currently: 127.0.0.1 and PORT is 1917
    */
    if ((rv = getaddrinfo(FLIGHTGEAR_IP, PORT, &hints, &servinfo)) != 0) {
        printf("getaddrinfo failed! rv = %d \n",rv);
        printf("error def: %s\n", gai_strerror(rv));
        return -1;
    } else {
        printf("addr info obtained!\n");
    }

    /*
        error check and initialize obtaining sockfd
    */
    if ((sockfd = socket(servinfo->ai_family, servinfo->ai_socktype, servinfo->ai_protocol)) == -1) {
        printf("socket file descriptor init failed\n");
        printf("error def: %s\n", gai_strerror(sockfd));
        return 1;
    } else {
        printf("socket file descriptor obtained!\n");
    }

    /*
        error check and intialize connection to fg server
    */
    if ((rv = connect(sockfd, servinfo->ai_addr, servinfo->ai_addrlen)) < 0) {
        close(sockfd);
        printf("connection failed to open\n");
        printf("error number is: %d def: %s\n", errno, gai_strerror(errno));
        return 1;
    } else {
        printf("no error in datagram socket connect!\n");
    }
    return 0;
}
 
int main()
{ 
    // 创建一个新的环境（苏雨）
    void* context = zmq_ctx_new();
    assert(context != NULL);
    // 该环境中只允许有一个socket的存在（苏雨） 
    int ret = zmq_ctx_set(context, ZMQ_MAX_SOCKETS, 1);
    assert(ret == 0); 
    // 创建一个订阅者（苏雨）
    void* subscriber = zmq_socket(context, ZMQ_SUB);
    assert(subscriber != NULL);
    // 连接到服务器（苏雨）
    ret = zmq_connect(subscriber, "tcp://localhost:5555");
    assert(ret == 0);
    // 必须添加该语句对消息滤波，否则接受不到消息（苏雨）
    ret = zmq_setsockopt(subscriber, ZMQ_SUBSCRIBE, "", 0);
    assert(ret == 0);
    // 消息缓冲区（苏雨）
    char buf[20];
    udp_connect_setup();
    
    
    
    
    //开始循环，接收数据（苏雨）        
    while(1)
    {
        
        // 接收消息，非堵塞式（苏雨）
        ret = zmq_recv(subscriber, buf, 16, ZMQ_DONTWAIT);
        if (ret != -1)/// 打印消息（苏雨）
        {
            buf[ret] = '\0';
            //printf("%s\n", buf);            
            if (init == 1)
            {
                //如果第一次执行程序，则定义数组并初始化（苏雨）
                lat = {};
                lon = {};
                alt = {};
                tas = {};
                trk = {};                	
                //初始化完成关闭初始化开关（苏雨）
                init = 0;
            }
            //c语言没有string类型而c++有，c语言中字符串是通过字符指针来间接实现（苏雨）
            string s1;
            string s2;
            string s3;
            string s4;
            string s5;
            string s6;
            string s7;            
            s1 = buf;
            //c的双引号需要转义符\（苏雨）
            s2 = "\"Position\"";
            s3 = "\"Lat\"";
            s4 = "\"Lon\"";
            s5 = "\"Alt\"";
            s6 = "\"Tas\"";
            s7 = "\"Trk\"";
            
            //c++字符串可以直接比较（苏雨）
            if (s1 == s2)
            {
                //在这里向量获得了所有飞机的参数（苏雨）
                lat_save = lat;
                lon_save = lon;
                alt_save = alt;
                tas_save = tas;
                trk_save = trk;
                //std::cout<<lat_save[1]<<" "<<setprecision(20)<<std::endl;

                //int a;
                //a = lat_save.size();
                //printf("%d\n",a);

                //初始化向量，用来存储采样周期内所有飞机位置信息（苏雨）
                lat = {};
                lon = {};
                alt = {};
                tas = {};
                trk = {};
                latlonalt = 0;
            }
            else
            {
                if (s1 == s3)
                {
                    latlonalt = 1;
                }
                else if (s1 == s4)
                {
                    latlonalt = 2;
                }
                else if (s1 == s5)
                {
                    latlonalt = 3;
                }
                else if (s1 == s6)
                {
                    latlonalt = 4;
                }                                
                else if (s1 == s7)
                {
                    latlonalt = 5;
                } 
                else
                {
                    if (latlonalt == 1)
                    {
                        //将字符串转为双精度浮点数，八位（苏雨）
                        double transfer;
                        transfer = atof(buf);
                        //向向量末尾插信息（苏雨）
                        lat.push_back(transfer);
                        latlonalt = 0;
                    }
                    if (latlonalt == 2)
                    {
                        //将字符串转为双精度浮点数，八位（苏雨）
                        double transfer;
                        transfer = atof(buf);
                        //向向量末尾插信息（苏雨）
                        lon.push_back(transfer);
                        latlonalt = 0;
                    }
                    if (latlonalt == 3)
                    {
                        //将字符串转为双精度浮点数，八位（苏雨）
                        double transfer;
                        transfer = atof(buf);
                        //向向量末尾插信息（苏雨）
                        alt.push_back(transfer);
                        latlonalt = 0;
                    }
                    if (latlonalt == 4)
                    {
                        //将字符串转为双精度浮点数，八位（苏雨）
                        double transfer;
                        transfer = atof(buf);
                        //向向量末尾插信息（苏雨）
                        tas.push_back(transfer);
                        latlonalt = 0;
                    }                                          
                    if (latlonalt == 5)
                    {
                        //将字符串转为双精度浮点数，八位（苏雨）
                        double transfer;
                        transfer = atof(buf);
                        //向向量末尾插信息（苏雨）
                        trk.push_back(transfer);
                        latlonalt = 0;
                    }                                       
                }                                                                               
            }
            //std::copy(lat_save.begin(), lat_save.end(), std::ostream_iterator<double>(cout, " "));
            //printf("\n");
            //std::cout<<lat_save[0]<<" "<<setprecision(20)<<std::endl;                       
        }
        //sleep(1);
        
                int size;
                size = lat_save.size();
                //一定不要忘了初始化！！！！！！！！！（苏雨）
                data_len = 0;
                //printf("%d\n",size); 
                //注意！这里必须要判断向量是否为空，在前几个循环中，向量还是空的，不能直接调用里面的值，会导致程序奔溃！（苏雨）
                if (size>0)
                {
                    //std::cout<<lat_save[0]<<" "<<setprecision(20)<<std::endl;
                    //从这里，就可以构建向FG发送数据的部分了！（苏雨）
                    //定义给FG发送的AI飞机序号,以便后期增加功能（苏雨）
                    int aircraft_order = 0;
                    //定义字符串，用来存储需要发给FG的信息（苏雨）
                    char message[1024] = {};
                    while (aircraft_order<size)
                    {
                        double alt;
                        double lat;
                        double lon;
                        double tas;
                        double trk;
                        alt = alt_save[aircraft_order];
                        lat = lat_save[aircraft_order];
                        lon = lon_save[aircraft_order];
                        tas = tas_save[aircraft_order];
                        trk = trk_save[aircraft_order];
                        char lateral_mode[32] = "rudd";
                        //请注意！FG里面速度单位是kt，而BS里面速度单位是m/s！！！也就是说tas单位是m/s！！！所以这里要加入单位转换（苏雨）
                        //1kt=1n mile/h=1.852km/h  1m/s=3.6km/h
                        double convert;
                        convert = tas*(3.6/1.852);                                                            
                        msg_len = sprintf(send_data, "%.2f\t%.2f\t%.2f\t%s\t",convert,alt,trk,lateral_mode);
                        data_len = data_len + msg_len;                        
                        strcat(message,send_data);
                        aircraft_order++;                                               
                    }

                    strcat(message,"\n");
                    data_len = data_len + 1;                  
                    printf("%s", message); 
                    //printf("\n%d\n", data_len);                                                          
                    send(sockfd, message, data_len, 0);                    
                }                
                //double b;
                //b = lat_save[0];           
    }   
    return 0;    
}

